﻿#include "sigma_points.h"
#include "../comm_define.h"

MerweScaledSigmaPoints::MerweScaledSigmaPoints(int n_, double alpha_, double beta_, double kappa_)
	:n(n_), alpha(alpha_), beta(beta_), kappa(kappa_)
{

}
MerweScaledSigmaPoints::~MerweScaledSigmaPoints()
{
}

int MerweScaledSigmaPoints::num_sigmas() const {
	return 2 * n + 1;
}

std::vector<VectorXd> MerweScaledSigmaPoints::sigma_points(VectorXd X, MatrixXd P) {
	double  lambda_ = POW2(alpha) * (n + kappa) - n;
	Eigen::MatrixXd U = ((lambda_ + n)*P).llt().matrixL(); //LLT分解

	std::vector<VectorXd> sigmas;
	sigmas.resize(num_sigmas());

	sigmas[0] = X;
	for (int k = 0; k < n; k++) {
		sigmas[k + 1] = X + U.col(k);
		sigmas[n + k + 1] = X - U.col(k);
	}
	return sigmas;
}

SigmaPointsWeights_t MerweScaledSigmaPoints::compute_weights()
{
	SigmaPointsWeights_t weights;
	weights.Wc.resize(num_sigmas());
	weights.Wm.resize(num_sigmas());

	double  lambda_ = POW2(alpha) * (n + kappa) - n;
	double c = 0.5 / (n + lambda_);
	for (size_t i = 0; i < num_sigmas(); i++) {
		weights.Wc[i] = weights.Wm[i] = c;
	}
	weights.Wm[0] = lambda_ / (n + lambda_);
	weights.Wc[0] = lambda_ / (n + lambda_) + (1 - POW2(alpha) + beta);

	return weights;
}
